/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.input;

import com.team1160.breakaway.api.Constants;
import com.team1160.breakaway.api.MotorSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;

/**
 * InputReader performs input controller read
 * and creates a new instance of InputState for
 * each invocation.
 *
 * This implementation assumes tank drive with
 * trigger and dribble buttons on joystick.  A
 * different configuration of driver controls
 * requires updates to this and model.
 * 
 * @author nttoole
 */
public class InputReader {

    DriverStation driverStation;
    InputState currentState;
    Joystick joystickA;
    Joystick joystickB;

    //---------------------------------------------------------------------
    
    public InputReader(DriverStation driverStation,
                       Joystick joystickA,
                       Joystick joystickB)
    {
        this.driverStation = driverStation;
        this.joystickA     = joystickA;
        this.joystickB     = joystickB;
        init();
    }

    //---------------------------------------------------------------------

    protected void init()
    {
        if (this.driverStation == null)
        {
           this.driverStation = DriverStation.getInstance();
        }
        if (this.joystickA == null)
        {
            this.joystickA = new Joystick(Constants.JOYSTICK_LEFT_PORT_ID);
        }
        if (this.joystickB == null)
        {
            this.joystickB = new Joystick(Constants.JOYSTICK_RIGHT_PORT_ID);
        }

    }

    //---------------------------------------------------------------------

    public InputState readInput()
    {
        InputState newState = new InputState();


        handleJoysticks(newState);
        handleButtons(newState);

        this.currentState = newState;
        return newState;

    }

    //---------------------------------------------------------------------

    public InputState getState()
    {
        return this.currentState;
    }

    //---------------------------------------------------------------------

    protected void handleJoysticks(InputState state)
    {
        //joystick axis values
        double lValue = this.joystickA.getRawAxis(Joystick.AxisType.kY.value);
        double rValue = this.joystickB.getRawAxis(Joystick.AxisType.kY.value);

        MotorSpeeds speeds = new MotorSpeeds(lValue, rValue);
        state.setMotorSpeeds(speeds);

        //joystick buttons

        //trigger
        boolean lBool, rBool;
        lBool = this.joystickA.getRawButton(
                        Constants.JOYSTICK_TRIGGER_BUTTON_ID);
        rBool = this.joystickB.getRawButton(
                        Constants.JOYSTICK_TRIGGER_BUTTON_ID);
        state.setKickerPressed(lBool || rBool);

        //dribbler
        lBool = this.joystickA.getRawButton(
                        Constants.JOYSTICK_DRIBBLER_BUTTON_ID);
        rBool = this.joystickB.getRawButton(
                        Constants.JOYSTICK_DRIBBLER_BUTTON_ID);
        state.setDribblerPressed(lBool || rBool);

    }

    //---------------------------------------------------------------------

    protected void handleButtons(InputState state)
    {
       //TODO - implement?
    }

    //---------------------------------------------------------------------
}
